#include <QDebug>

#include "mavlinkhandler.h"

///
/// \brief MavlinkHandler::MavlinkHandler
///
MavlinkHandler::MavlinkHandler()
{
}

///
/// \brief MavlinkHandler::~MavlinkHandler
///
MavlinkHandler::~MavlinkHandler()
{
    _should_exit = true;

    {
        std::lock_guard<std::mutex> lock(_connections_mutex);
        _connections.clear();
    }
}

///
/// \brief MavlinkHandler::get_target_system_id
/// \param message
/// \return
///
uint8_t MavlinkHandler::get_target_system_id(const mavlink_message_t &message)
{
    // Checks whether connection knows target system ID by extracting target system if set.
    const mavlink_msg_entry_t* meta = mavlink_get_msg_entry(message.msgid);

    if (meta == nullptr || !(meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM)) {
        return 0;
    }

    // Don't look at the target system offset if it is outside of the payload length.
    // This can happen if the fields are trimmed.
    if (meta->target_system_ofs >= message.len) {
        return 0;
    }

    return (_MAV_PAYLOAD(&message))[meta->target_system_ofs];
}

///
/// \brief MavlinkHandler::get_target_component_id
/// \param message
/// \return
///
uint8_t MavlinkHandler::get_target_component_id(const mavlink_message_t &message)
{
    // Checks whether connection knows target system ID by extracting target system if set.
    const mavlink_msg_entry_t* meta = mavlink_get_msg_entry(message.msgid);

    if (meta == nullptr || !(meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT)) {
        return 0;
    }

    // Don't look at the target component offset if it is outside of the payload length.
    // This can happen if the fields are trimmed.
    if (meta->target_component_ofs >= message.len) {
        return 0;
    }

    return (_MAV_PAYLOAD(&message))[meta->target_system_ofs];
}


///
/// \brief MavlinkHandler::send_message
/// \param message
/// \return
///
bool MavlinkHandler::send_message(mavlink_message_t &message)
{
    if (_message_logging_on) {
        qDebug() << "Sending message " << message.msgid << " from "
                   << static_cast<int>(message.sysid) << "/" << static_cast<int>(message.compid);
    }

    std::lock_guard<std::mutex> lock(_connections_mutex);

    uint8_t successful_emissions = 0;
    for (auto it = _connections.begin(); it != _connections.end(); ++it) {
        const uint8_t target_system_id = get_target_system_id(message);

        if (target_system_id != 0 && !(**it).has_system_id(target_system_id)) {
            continue;
        }

        if ((**it).send_message(message)) {
            successful_emissions++;
        }
    }

    if (successful_emissions == 0) {
        qDebug() << "Sending message failed";
        return false;
    }

    return true;
}

///
/// \brief MavlinkHandler::receive_message
/// \param message
/// \param connection
///
void MavlinkHandler::receive_message(mavlink_message_t& message, Connection* connection)
{
    UNUSED(connection);

    if (_message_logging_on) {
        qDebug() << "Processing message " << message.msgid << " from "
                   << static_cast<int>(message.sysid) << "/" << static_cast<int>(message.compid);
    }

    // Don't ever create a system with sysid 0.
    if (message.sysid == 0) {
        if (_message_logging_on) {
            qDebug() << "Ignoring message with sysid == 0";
        }
        return;
    }

    if (_should_exit) {
        // Don't try to call at() if systems have already been destroyed
        // in descructor.
        return;
    }

    // Process the meaasge,each register call-back function will be invoked.
    mMavMsgHandler.process_message(message);
}


///
/// \brief MavlinkHandler::add_connection
/// \param new_connection
///
void MavlinkHandler::add_connection(std::shared_ptr<Connection> new_connection)
{
    std::lock_guard<std::mutex> lock(_connections_mutex);
    _connections.push_back(new_connection);
}

///
/// \brief MavlinkHandler::add_udp_connection
/// \param local_ip
/// \param local_port
/// \return
///
ConnectionResult MavlinkHandler::add_udp_connection(const std::string &local_ip, int local_port)
{
    auto new_conn = std::make_shared<UdpConnection>(
        std::bind(&MavlinkHandler::receive_message, this, std::placeholders::_1, std::placeholders::_2),
        local_ip,
        local_port);
    if (!new_conn) {
        return ConnectionResult::ConnectionError;
    }
    ConnectionResult ret = new_conn->start();
    if (ret == ConnectionResult::Success) {
        add_connection(new_conn);
    }
    return ret;
}

///
/// \brief MavlinkHandler::add_tcp_connection
/// \param remote_ip
/// \param remote_port
/// \return
///
ConnectionResult MavlinkHandler::add_tcp_connection(const std::string &remote_ip, int remote_port)
{
    auto new_conn = std::make_shared<TcpConnection>(
        std::bind(&MavlinkHandler::receive_message, this, std::placeholders::_1, std::placeholders::_2),
        remote_ip,
        remote_port);
    if (!new_conn) {
        return ConnectionResult::ConnectionError;
    }
    ConnectionResult ret = new_conn->start();
    if (ret == ConnectionResult::Success) {
        add_connection(new_conn);
    }
    return ret;
}

///
/// \brief MavlinkHandler::add_serial_connection
/// \param dev_path
/// \param baudrate
/// \param flow_control
/// \return
///
ConnectionResult MavlinkHandler::add_serial_connection(const std::string &dev_path, int baudrate, bool flow_control)
{
    auto new_conn = std::make_shared<SerialConnection>(
        std::bind(&MavlinkHandler::receive_message, this, std::placeholders::_1, std::placeholders::_2),
        dev_path,
        baudrate,
        flow_control);
    if (!new_conn) {
        return ConnectionResult::ConnectionError;
    }
    ConnectionResult ret = new_conn->start();
    if (ret == ConnectionResult::Success) {
        add_connection(new_conn);
    }
    return ret;
}

